{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 摄像机标定"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2             "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 读入文件"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 从文件中读取三维点坐标\n",
    "def read_3d_points(filename):\n",
    "    points_3d = []\n",
    "    with open(filename, 'r') as file:\n",
    "        for line in file:\n",
    "            parts = line.split()\n",
    "            if len(parts) == 3:\n",
    "                points_3d.append([float(part) for part in parts])\n",
    "    return np.array(points_3d)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[-1.4889  ,  1.09408 , -5.89588 ],\n",
       "       [-1.13932 ,  0.935522, -5.62824 ],\n",
       "       [ 1.09404 ,  0.738447, -4.53106 ],\n",
       "       ...,\n",
       "       [-2.49859 , -0.117172, -6.76897 ],\n",
       "       [-2.51002 , -0.144305, -6.7266  ],\n",
       "       [-1.59536 , -1.37243 , -3.78376 ]])"
      ]
     },
     "execution_count": 24,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "points_3D = read_3d_points('house.p3d')\n",
    "points_3D"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 设定相机内外参数"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 设定SVD分解的参数"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "f_svd = 500  # 焦距\n",
    "c_x_svd, c_y_svd = 320, 240  # 主点坐标\n",
    "K_svd = np.array([[f_svd, 0, c_x_svd],\n",
    "              [0, f_svd, c_y_svd],\n",
    "              [0, 0, 1]])\n",
    "\n",
    "R_svd = np.eye(3)  # 旋转矩阵\n",
    "T_svd = np.array([0, 0, 100])  # 平移向量\n",
    "RT_svd = np.hstack((R_svd, T_svd.reshape(3, 1)))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 设定QR分解的参数"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "metadata": {},
   "outputs": [],
   "source": [
    "f_qr = 0  # 焦距\n",
    "c_x_qr, c_y_qr = 320, 240  # 主点坐标\n",
    "# K_qr = np.array([[f_qr, 0, c_x_qr],\n",
    "#               [0, f_qr, c_y_qr],\n",
    "#               [0, 0, 1]])\n",
    "\n",
    "K_qr = np.array([[-8.00005438e-01,6.00009249e-01,2.12213671e-04],\n",
    "                [-6.00004078e-01,-7.99996969e-01,-4.44961823e-03],\n",
    "                [-2.50001699e-03,-3.68701126e-03,1.00000000e+00]])\n",
    "\n",
    "R_qr = np.eye(3)  # 旋转矩阵\n",
    "T_qr = np.array([0, 0, 100])  # 平移向量\n",
    "RT_qr = np.hstack((R_qr, T_qr.reshape(3, 1)))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 投影三维点到二维图像"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### SVD"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "points_2D_projected_svd = K_svd @ RT_svd @ np.vstack((points_3D.T, np.ones((1, points_3D.shape[0]))))\n",
    "points_2D_projected_svd = points_2D_projected_svd[:2] / points_2D_projected_svd[2]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### QR"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {},
   "outputs": [],
   "source": [
    "points_2D_projected_qr = K_qr @ RT_qr @ np.vstack((points_3D.T, np.ones((1, points_3D.shape[0]))))\n",
    "points_2D_projected_qr = points_2D_projected_qr[:2] / points_2D_projected_qr[2]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 使用SVD分解和QR分解估计相机参数"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 使用SVD分解"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [],
   "source": [
    "def estimate_camera_params_svd(points_3D, points_2D):\n",
    "    A = np.zeros((2 * points_3D.shape[0], 12))\n",
    "    for i in range(points_3D.shape[0]):\n",
    "        X, Y, Z = points_3D[i]\n",
    "        u, v = points_2D[i]\n",
    "        A[2*i] = [X, Y, Z, 1, 0, 0, 0, 0, -u*X, -u*Y, -u*Z, -u]\n",
    "        A[2*i+1] = [0, 0, 0, 0, X, Y, Z, 1, -v*X, -v*Y, -v*Z, -v]\n",
    "\n",
    "    U, S, Vt = np.linalg.svd(A)\n",
    "    P = Vt[-1].reshape(3, 4)\n",
    "\n",
    "    K = P[:, :3]\n",
    "    R = np.eye(3)  # 初始化R\n",
    "    T = np.zeros(3)  # 初始化T\n",
    "\n",
    "    K /= K[2, 2]\n",
    "    T = np.linalg.inv(K) @ P[:, 3]\n",
    "\n",
    "    return K, R, T\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 使用QR分解"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 使用QR分解估计相机参数\n",
    "def estimate_camera_params_qr(points_3D, points_2D):\n",
    "    A = np.zeros((2 * points_3D.shape[0], 12))\n",
    "    for i in range(points_3D.shape[0]):\n",
    "        X, Y, Z = points_3D[i]\n",
    "        u, v = points_2D[i]\n",
    "        A[2*i] = [X, Y, Z, 1, 0, 0, 0, 0, -u*X, -u*Y, -u*Z, -u]\n",
    "        A[2*i+1] = [0, 0, 0, 0, X, Y, Z, 1, -v*X, -v*Y, -v*Z, -v]\n",
    "\n",
    "    U, S, Vt = np.linalg.svd(A)\n",
    "    P = Vt[-1].reshape(3, 4)\n",
    "\n",
    "    Q, R_qr = np.linalg.qr(P[:, :3])\n",
    "    K = Q\n",
    "    T = np.linalg.inv(K) @ P[:, 3]\n",
    "\n",
    "    K /= K[2, 2]\n",
    "\n",
    "    return K, R_qr, T"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 估计相机参数"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### SVD分解"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "设定的内参矩阵 K:\n",
      " [[500   0 320]\n",
      " [  0 500 240]\n",
      " [  0   0   1]]\n",
      "设定的旋转矩阵 R:\n",
      " [[1. 0. 0.]\n",
      " [0. 1. 0.]\n",
      " [0. 0. 1.]]\n",
      "设定的平移向量 T:\n",
      " [  0   0 100]\n",
      "\n",
      "估计得到的内参矩阵 K:\n",
      " [[ 5.00000000e+02  3.06330700e-10  3.20000000e+02]\n",
      " [-3.58660303e-10  5.00000000e+02  2.40000000e+02]\n",
      " [-9.19189262e-13 -4.42760563e-13  1.00000000e+00]]\n",
      "估计得到的旋转矩阵 R:\n",
      " [[1. 0. 0.]\n",
      " [0. 1. 0.]\n",
      " [0. 0. 1.]]\n",
      "估计得到的平移向量 T:\n",
      " [ 7.74987713e-16 -3.27429056e-16  2.49947673e-03]\n"
     ]
    }
   ],
   "source": [
    "estimated_K, estimated_R, estimated_T = estimate_camera_params_svd(points_3D, points_2D_projected_svd.T)\n",
    "\n",
    "# 打印结果\n",
    "print(\"设定的内参矩阵 K:\\n\", K_svd)\n",
    "print(\"设定的旋转矩阵 R:\\n\", R_svd)\n",
    "print(\"设定的平移向量 T:\\n\", T_svd)\n",
    "print(\"\\n估计得到的内参矩阵 K:\\n\", estimated_K)\n",
    "print(\"估计得到的旋转矩阵 R:\\n\", estimated_R)\n",
    "print(\"估计得到的平移向量 T:\\n\", estimated_T)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### QR分解"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "设定的内参矩阵 K:\n",
      " [[-8.00005438e-01  6.00009249e-01  2.12213671e-04]\n",
      " [-6.00004078e-01 -7.99996969e-01 -4.44961823e-03]\n",
      " [-2.50001699e-03 -3.68701126e-03  1.00000000e+00]]\n",
      "设定的旋转矩阵 R:\n",
      " [[1. 0. 0.]\n",
      " [0. 1. 0.]\n",
      " [0. 0. 1.]]\n",
      "设定的平移向量 T:\n",
      " [  0   0 100]\n",
      "\n",
      "估计得到的内参矩阵 K:\n",
      " [[-8.00005438e-01  6.00009249e-01  2.12213672e-04]\n",
      " [-6.00004078e-01 -7.99996970e-01 -4.44961823e-03]\n",
      " [-2.50001699e-03 -3.68701126e-03  1.00000000e+00]]\n",
      "估计得到的旋转矩阵 R:\n",
      " [[-9.99850034e-03  6.71752411e-12 -2.73270096e-14]\n",
      " [ 0.00000000e+00 -9.99850033e-03 -2.50612858e-14]\n",
      " [ 0.00000000e+00  0.00000000e+00 -9.99850034e-03]]\n",
      "估计得到的平移向量 T:\n",
      " [-2.73251430e-12 -2.50763733e-12 -9.99850034e-01]\n"
     ]
    }
   ],
   "source": [
    "estimated_K_qr, estimated_R_qr, estimated_T_qr = estimate_camera_params_qr(points_3D, points_2D_projected_qr.T)\n",
    "\n",
    "# 打印结果\n",
    "print(\"设定的内参矩阵 K:\\n\", K_qr)\n",
    "print(\"设定的旋转矩阵 R:\\n\", R_qr)\n",
    "print(\"设定的平移向量 T:\\n\", T_qr)\n",
    "print(\"\\n估计得到的内参矩阵 K:\\n\", estimated_K_qr)\n",
    "print(\"估计得到的旋转矩阵 R:\\n\", estimated_R_qr)\n",
    "print(\"估计得到的平移向量 T:\\n\", estimated_T_qr)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
